package com.dji.activationDemo.messaging;

import static dji.keysdk.FlightControllerKey.HOME_LOCATION_LATITUDE;
import static dji.keysdk.FlightControllerKey.HOME_LOCATION_LONGITUDE;

import androidx.annotation.NonNull;
import androidx.annotation.Nullable;

import com.dji.activationDemo.Constant;
import com.dji.activationDemo.uitls.ModuleVerificationUtil;
import com.dji.activationDemo.uitls.RandomNum;
import com.dji.activationDemo.websocket.WsManager;

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

import dji.common.error.DJIError;
import dji.common.error.DJIWaypointV2Error;
import dji.common.flightcontroller.FlightMode;
import dji.common.gimbal.Rotation;
import dji.common.gimbal.RotationMode;
import dji.common.mission.waypoint.Waypoint;
import dji.common.mission.waypoint.WaypointAction;
import dji.common.mission.waypoint.WaypointActionType;
import dji.common.mission.waypoint.WaypointMission;
import dji.common.mission.waypoint.WaypointMissionDownloadEvent;
import dji.common.mission.waypoint.WaypointMissionExecutionEvent;
import dji.common.mission.waypoint.WaypointMissionFinishedAction;
import dji.common.mission.waypoint.WaypointMissionFlightPathMode;
import dji.common.mission.waypoint.WaypointMissionGotoWaypointMode;
import dji.common.mission.waypoint.WaypointMissionHeadingMode;
import dji.common.mission.waypoint.WaypointMissionState;
import dji.common.mission.waypoint.WaypointMissionUploadEvent;
import dji.common.mission.waypoint.WaypointTurnMode;
import dji.common.mission.waypointv2.Action.ActionDownloadEvent;
import dji.common.mission.waypointv2.Action.ActionExecutionEvent;
import dji.common.mission.waypointv2.Action.ActionState;
import dji.common.mission.waypointv2.Action.ActionTypes;
import dji.common.mission.waypointv2.Action.ActionUploadEvent;
import dji.common.mission.waypointv2.Action.WaypointActuator;
import dji.common.mission.waypointv2.Action.WaypointAircraftControlParam;
import dji.common.mission.waypointv2.Action.WaypointAircraftControlStartStopFlyParam;
import dji.common.mission.waypointv2.Action.WaypointCameraActuatorParam;
import dji.common.mission.waypointv2.Action.WaypointCameraCustomNameParam;
import dji.common.mission.waypointv2.Action.WaypointGimbalActuatorParam;
import dji.common.mission.waypointv2.Action.WaypointReachPointTriggerParam;
import dji.common.mission.waypointv2.Action.WaypointTrigger;
import dji.common.mission.waypointv2.Action.WaypointV2Action;
import dji.common.mission.waypointv2.Action.WaypointV2AssociateTriggerParam;
import dji.common.mission.waypointv2.WaypointV2;
import dji.common.mission.waypointv2.WaypointV2Mission;
import dji.common.mission.waypointv2.WaypointV2MissionDownloadEvent;
import dji.common.mission.waypointv2.WaypointV2MissionExecutionEvent;
import dji.common.mission.waypointv2.WaypointV2MissionState;
import dji.common.mission.waypointv2.WaypointV2MissionTypes;
import dji.common.mission.waypointv2.WaypointV2MissionUploadEvent;
import dji.common.model.LocationCoordinate2D;
import dji.common.util.CommonCallbacks;
import dji.keysdk.FlightControllerKey;
import dji.keysdk.KeyManager;
import dji.sdk.flightcontroller.FlightController;
import dji.sdk.mission.MissionControl;
import dji.sdk.mission.waypoint.WaypointMissionOperator;
import dji.sdk.mission.waypoint.WaypointMissionOperatorListener;
import dji.sdk.mission.waypoint.WaypointV2ActionListener;
import dji.sdk.mission.waypoint.WaypointV2MissionOperatorListener;

public class WaypointMissionRes {
    private FlightController flightController = ModuleVerificationUtil.getFlightController();

    // Global variables for waypointV2Mission
    private WaypointMissionOperator waypointMissionOperator = MissionControl.getInstance().getWaypointMissionOperator();


    RandomNum randomNum = new RandomNum();

    String[] waypoints;
    int num;
    int taskLength;
    int start = 1;
    int end;

    int angle;
    String sign;


    //返航
    public void goHome(String sign) {
//        this.sign = sign;
        flightController.startGoHome(new CommonCallbacks.CompletionCallback() {
            @Override
            public void onResult(DJIError djiError) {
                if (sign != null) {
                    if (djiError != null) {
                        WsManager.ws.sendText("error*" + sign + "*" + djiError.getDescription());
                    } else {
                        WsManager.ws.sendText("received*" + sign);
                    }
                }
            }
        });
    }

    //恢复任务
    public void resumeMission(String sign) {
//        this.sign = sign;
        waypointMissionOperator.resumeMission(new CommonCallbacks.CompletionCallback() {
            @Override
            public void onResult(DJIError djiError) {
                if (djiError != null) {
                    WsManager.ws.sendText("error*" + sign + "*" + djiError.getDescription());
                } else {
                    WsManager.ws.sendText("received*" + sign);
                }
            }
        });
    }

    //暂停任务
    public void pauseMission(String sign) {
//        this.sign = sign;
        waypointMissionOperator.pauseMission(new CommonCallbacks.CompletionCallback() {
            @Override
            public void onResult(DJIError djiError) {
                if (djiError != null) {
                    WsManager.ws.sendText("error*" + sign + "*" + djiError.getDescription());
                } else {
                    WsManager.ws.sendText("received*" + sign);
                }
            }
        });
    }

    //停止任务
    public void stopMission(String sign) {
        this.sign = sign;
        waypointMissionOperator.stopMission(new CommonCallbacks.CompletionCallback() {
            @Override
            public void onResult(DJIError djiError) {
                if (sign != null) {
                    if (djiError != null) {
                        WsManager.ws.sendText("error*" + sign + "*" + djiError.getDescription());
                    } else {
                        WsManager.ws.sendText("received*" + sign);
                    }
                }
            }
        });
//        waypointMissionOperator.removeListener(Constant.waypointMissionOperatorListener);
    }

    public void photogrammetryTask(String paramString, String sign) {
        //编辑任务点航向
        WaypointMission waypointMission = createWaypointMission(paramString);

        //开始执行任务
        loadMission(waypointMission, sign);
    }

    //开始执行任务
    //加载任务
    private void loadMission(WaypointMission waypointMission, String sign) {

        DJIError djiError = waypointMissionOperator.loadMission(waypointMission);
        if (djiError == null) {
            uploadMission(sign);
            System.out.println("Mission is loaded successfully");
        } else {
            WsManager.ws.sendText("error*" + sign + "*加载任务失败：" + djiError.getDescription());
            System.out.println("加载任务报错了:" + djiError.toString());
        }

    }

    //上传任务
    private void uploadMission(String sign) {
        if (WaypointMissionState.READY_TO_RETRY_UPLOAD.equals(waypointMissionOperator.getCurrentState()) || WaypointMissionState.READY_TO_UPLOAD.equals(waypointMissionOperator.getCurrentState())) {
            waypointMissionOperator.uploadMission(new CommonCallbacks.CompletionCallback() {
                @Override
                public void onResult(DJIError djiError) {
                    if (djiError == null) {
                        System.out.println("任务上传成功");
                        startMission(sign);
                    } else {
                        WsManager.ws.sendText("error*" + sign + "*上传任务失败：" + djiError.getDescription());
                        System.out.println("上传任务报错：" + djiError.toString());
                    }
                }
            });
        } else {
            System.out.println("Wait for mission to be loaded");
        }
    }

    //开始执行任务
    private void startMission(String sign) {
        if (WaypointMissionState.READY_TO_EXECUTE.equals(waypointMissionOperator.getCurrentState())) {
            System.out.println("任务上传完成，开始执行");
        } else {
            System.out.println("等待上传完成");
            try {
                Thread.sleep(4000);
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        }
        waypointMissionOperator.startMission(new CommonCallbacks.CompletionCallback() {
            @Override
            public void onResult(DJIError djiError) {
                if (djiError != null) {
                    WsManager.ws.sendText("error*" + sign + "*执行报错：" + djiError.getDescription());
                    System.out.println("执行报错:" + djiError.toString());
                } else {
                    WsManager.ws.sendText("received*" + sign);
                    System.out.println("开始执行任务");
                }
            }
        });

    }

    //添加航线点
    private WaypointMission createWaypointMission(String paramString) {
        WaypointMission.Builder builder = new WaypointMission.Builder();
        builder.autoFlightSpeed(5f);
        builder.maxFlightSpeed(10f);
        builder.setExitMissionOnRCSignalLostEnabled(false);
        builder.finishedAction(WaypointMissionFinishedAction.GO_HOME);//任务完成：返航
        builder.flightPathMode(WaypointMissionFlightPathMode.NORMAL);//航线模式：直线
        builder.gotoFirstWaypointMode(WaypointMissionGotoWaypointMode.SAFELY);//安全前往航点
//        builder.setPointOfInterest(new LocationCoordinate2D(15, 15));//将无人机航向固定到兴趣点
        builder.headingMode(WaypointMissionHeadingMode.AUTO);//航向模式:与飞行方向一致

        List<Waypoint> waypointList = new ArrayList<>();

        String[] wayList = paramString.split("\\|");
        for (int i = 1; i < wayList.length; i++) {
            String[] aa = wayList[i].split(",");
            double baseLongitude = Double.parseDouble(aa[0]);
            double baseLatitude = Double.parseDouble(aa[1]);
            float baseAltitude = Float.parseFloat(aa[2]);

            Waypoint waypoint = new Waypoint(baseLatitude, baseLongitude, baseAltitude);
            waypoint.turnMode = WaypointTurnMode.CLOCKWISE;
            waypoint.addAction(new WaypointAction(WaypointActionType.GIMBAL_PITCH, -90));
            waypoint.addAction(new WaypointAction(WaypointActionType.START_TAKE_PHOTO, 0));
            waypointList.add(waypoint);
        }
        builder.waypointList(waypointList).waypointCount(waypointList.size());
        return builder.build();
    }

    public void photogrammetryPitchTask(String paramString, String sign, int angle) {
        waypoints = paramString.split("\\|");
        WaypointMission waypointMission = null;

        //编辑任务点航向
        waypointMission = createPitchWaypointMission(paramString, angle);
        //开始执行任务
        loadMission(waypointMission, sign);
    }

    public void photogrammetryPitchTask1(String paramString, String sign, int angle) {
        start = 1;
        if (Constant.waypointMissionOperatorListener == null)
            setUpListener();
        waypoints = paramString.split("\\|");
        WaypointMission waypointMission = null;
        this.angle = angle;
        this.sign = sign;

        num = (int) Math.ceil((waypoints.length - 1) / (double) 99);
        System.out.println("任务次数：" + num);
        if (num <= 1) {
            end = waypoints.length - 1;
            waypointMission = createPitchWaypointMission1(angle);
            num--;
        } else {
            taskLength = (waypoints.length - 1) / num;
            end = taskLength;
            waypointMission = createPitchWaypointMission1(angle);
            num--;
        }
        //开始执行任务
        loadMission(waypointMission, sign);
    }


    //添加俯视角可变的航线点
    private WaypointMission createPitchWaypointMission(String paramString, int angle) {
        WaypointMission.Builder builder = new WaypointMission.Builder();
        builder.autoFlightSpeed(5f);
        builder.maxFlightSpeed(10f);
        builder.setExitMissionOnRCSignalLostEnabled(false);
        builder.finishedAction(WaypointMissionFinishedAction.NO_ACTION);//任务完成：悬停
        builder.flightPathMode(WaypointMissionFlightPathMode.NORMAL);//航线模式：直线
        builder.gotoFirstWaypointMode(WaypointMissionGotoWaypointMode.SAFELY);//安全前往航点
        builder.headingMode(WaypointMissionHeadingMode.AUTO);//航向模式:与飞行方向一致

        List<Waypoint> waypointList = new ArrayList<>();

        String[] wayList = paramString.split("\\|");
        for (int i = 1; i < wayList.length; i++) {
            if (i == 1) {
                String[] aa = wayList[i].split(",");
                double baseLongitude = Double.parseDouble(aa[0]);
                double baseLatitude = Double.parseDouble(aa[1]);
                float baseAltitude = Float.parseFloat(aa[2]);

                Waypoint waypoint = new Waypoint(baseLatitude, baseLongitude, baseAltitude);
                waypoint.turnMode = WaypointTurnMode.CLOCKWISE;
                waypoint.addAction(new WaypointAction(WaypointActionType.GIMBAL_PITCH, angle));
                waypoint.addAction(new WaypointAction(WaypointActionType.START_TAKE_PHOTO, 0));
                waypointList.add(waypoint);
            } else {
                String[] aa = wayList[i].split(",");
                double baseLongitude = Double.parseDouble(aa[0]);
                double baseLatitude = Double.parseDouble(aa[1]);
                float baseAltitude = Float.parseFloat(aa[2]);

                Waypoint waypoint = new Waypoint(baseLatitude, baseLongitude, baseAltitude);
                waypoint.turnMode = WaypointTurnMode.CLOCKWISE;
                waypoint.addAction(new WaypointAction(WaypointActionType.START_TAKE_PHOTO, 0));
                waypointList.add(waypoint);
            }

        }
        builder.waypointList(waypointList).waypointCount(waypointList.size());
        return builder.build();
    }


    //规划航点
    private WaypointMission createPitchWaypointMission1(int angle) {
        WaypointMission.Builder builder = new WaypointMission.Builder();
        builder.autoFlightSpeed(5f);
        builder.maxFlightSpeed(10f);
        builder.setExitMissionOnRCSignalLostEnabled(false);
        builder.finishedAction(WaypointMissionFinishedAction.NO_ACTION);//任务完成：悬停
        builder.flightPathMode(WaypointMissionFlightPathMode.NORMAL);//航线模式：直线
        builder.gotoFirstWaypointMode(WaypointMissionGotoWaypointMode.SAFELY);//安全前往航点
        builder.headingMode(WaypointMissionHeadingMode.AUTO);//航向模式:与飞行方向一致
        List<Waypoint> waypointList = new ArrayList<>();
        for (int i = start; i <= end; i++) {
            String[] aa = waypoints[i].split(",");
            double baseLongitude = Double.parseDouble(aa[0]);
            double baseLatitude = Double.parseDouble(aa[1]);
            float baseAltitude = Float.parseFloat(aa[2]);

            Waypoint waypoint = new Waypoint(baseLatitude, baseLongitude, baseAltitude);
            waypoint.turnMode = WaypointTurnMode.CLOCKWISE;
            waypoint.addAction(new WaypointAction(WaypointActionType.GIMBAL_PITCH, angle));
            waypoint.addAction(new WaypointAction(WaypointActionType.START_TAKE_PHOTO, 0));
            waypointList.add(waypoint);
        }
        builder.waypointList(waypointList).waypointCount(waypointList.size());
        return builder.build();
    }


    public void setUpListener() {
        Constant.waypointMissionOperatorListener = new WaypointMissionOperatorListener() {
            @Override
            public void onDownloadUpdate(WaypointMissionDownloadEvent waypointMissionDownloadEvent) {
//                System.out.println("航线任务下载成功");
//                WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*航线任务下载成功");
            }

            @Override
            public void onUploadUpdate(WaypointMissionUploadEvent waypointMissionUploadEvent) {
//                System.out.println("航线任务上传成功");
//                WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*航线任务上传成功");
            }

            @Override
            public void onExecutionUpdate(WaypointMissionExecutionEvent waypointMissionExecutionEvent) {

            }

            @Override
            public void onExecutionStart() {
                WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*航线任务开始");
                System.out.println("监听到航点任务开始");
            }

            @Override
            public void onExecutionFinish(DJIError djiError) {
                if (djiError == null) {
                    System.out.println("任务还剩次数：" + num);
                    if (num == 0) {
                        System.out.println("航点结束");
                        WsManager.ws.sendText("frontend*" + randomNum.genRandomNum() + "*航线任务结束");
                    } else {
                        nextTask();
                    }
                } else {
                    System.out.println("结束失败:" + djiError.getDescription());
                }

            }
        };
        waypointMissionOperator.addListener(Constant.waypointMissionOperatorListener);
    }

    private WaypointMission createSingleWaypoint(double baseLongitude, double baseLatitude, float baseAltitude) {
        WaypointMission.Builder builder = new WaypointMission.Builder();
        builder.autoFlightSpeed(5f);
        builder.maxFlightSpeed(10f);
        builder.setExitMissionOnRCSignalLostEnabled(false);
        builder.finishedAction(WaypointMissionFinishedAction.NO_ACTION);//任务完成：悬停
        builder.flightPathMode(WaypointMissionFlightPathMode.NORMAL);//航线模式：直线
        builder.gotoFirstWaypointMode(WaypointMissionGotoWaypointMode.SAFELY);//安全前往航点
        builder.headingMode(WaypointMissionHeadingMode.AUTO);//航向模式:与飞行方向一致


        List<Waypoint> waypointList = new ArrayList<>();

        Waypoint waypoint = new Waypoint(baseLatitude, baseLongitude, baseAltitude);
        waypoint.turnMode = WaypointTurnMode.CLOCKWISE;
        waypointList.add(waypoint);
        builder.waypointList(waypointList).waypointCount(waypointList.size());
        return builder.build();
    }


    public void nextTask() {
        System.out.println("准备下一次任务");
        if (num == 1) {
            start = end + 1;
            end = waypoints.length - 1;
            num--;
        } else {
            start = end + 1;
            end = end + taskLength;
            num--;
        }
        WaypointMission waypointMission = createPitchWaypointMission1(angle);
        loadMission(waypointMission, sign);
    }







}
